package lego.jbv;

import java.util.Date;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
//import lejos.robotics.MirrorMotor;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;
import lejos.util.Delay;

/**
 * Demonstration of the Behavior subsumption classes.
 * 
 * Requires a wheeled vehicle with two independently controlled motors connected
 * to motor ports A and C, and a touch sensor connected to sensor port 1 and an
 * ultrasonic sensor connected to port 3;
 * 
 * @author Brian Bagnall and Lawrie Griffiths, modified by Roger Glassey
 * 
 */
public class BumperCar {
	static RegulatedMotor leftMotor = Motor.B;
	static RegulatedMotor rightMotor = Motor.C;

	// Use these definitions instead if your motors are inverted
	// static RegulatedMotor leftMotor = MirrorMotor.invertMotor(Motor.A);
	// static RegulatedMotor rightMotor = MirrorMotor.invertMotor(Motor.C);

	public static void main(String[] args) {
		leftMotor.setSpeed(400);
		rightMotor.setSpeed(400);
		Behavior b1 = new DriveForward();
		Behavior b2 = new DetectWall();
		Behavior b3 = new Escape();
		Behavior[] behaviorList = { b1, b2, b3 }; // Prioritied list from low to
													// high.
		Arbitrator arbitrator = new Arbitrator(behaviorList);
		LCD.drawString("Bumper Car", 0, 1);
		Button.waitForAnyPress();
		arbitrator.start();
	}
}

class DriveForward implements Behavior {

	private boolean _suppressed = false;

	public boolean takeControl() {
		return true; // this behavior always wants control.
	}

	public void suppress() {
		_suppressed = true;// standard practice for suppress methods
	}

	public void action() {
		_suppressed = false;
		BumperCar.leftMotor.forward();
		BumperCar.rightMotor.forward();
		while (!_suppressed) {
			Thread.yield(); // don't exit till suppressed
		}
		BumperCar.leftMotor.stop();
		BumperCar.leftMotor.stop();
	}
}

class DetectWall implements Behavior {
	private boolean _suppressed = false;

	public DetectWall() {
		touch = new TouchSensor(SensorPort.S1);
		sonar = new SonarSense();
		sonar.start();

	}

	public boolean takeControl() {

		return touch.isPressed() || sonar.getDistance() < 25;
	}

	public void suppress() {
		_suppressed = true;// standard practice for suppress methods
	}

	public void action() {
		_suppressed = false;
		
		long start = System.currentTimeMillis();
		
		while(!_suppressed && System.currentTimeMillis()-start < 1000)
		{
			BumperCar.leftMotor.backward();
			BumperCar.rightMotor.backward();
		}
		
		BumperCar.leftMotor.rotate(-360, true);// start Motor.A rotating										// backward
		BumperCar.rightMotor.rotate(-720, true); // rotate C farther to make the turn
		
		start = System.currentTimeMillis();
		
		while(!_suppressed && System.currentTimeMillis()-start < 1500 && BumperCar.rightMotor.isMoving() && BumperCar.leftMotor.isMoving())
		{
			Thread.yield();
		}
	}

	private TouchSensor touch;
	private SonarSense sonar;
}

class Escape implements Behavior {

	@Override
	public boolean takeControl() {
		return Button.ESCAPE.isDown();
	}

	@Override
	public void action() {
		System.exit(0);
	}

	@Override
	public void suppress() {
		// Since this is highest priority behavior, suppress will never be
		// called.
	}

}

class SonarSense extends Thread {
	private UltrasonicSensor sonar;
	private int distance;

	public SonarSense() {
		sonar = new UltrasonicSensor(SensorPort.S2);
	}

	public void run() {
		while (true) {
			sonar.ping();
			setDistance(sonar.getDistance());
			Delay.msDelay(200);
		}
	}

	public synchronized void setDistance(int value) {
		distance = value;
	}

	public synchronized int getDistance() {
		return distance;
	}
}
